import Agent from "./Agent";
import KdTree from "./KdTree";
import Obstacle from "./Obstacle";
import RVOMath from "./RVOMath";
import Vector2 from "./Vector2";

export class RVOSetting {
    /**代理对象总数 */
    public static agentCount = 300;
    /**代理对象之间的距离 */
    public static neighborDist = 35;
    /**代理对象的半径 */
    public static radius = 10;
    /**代理对象的最大移动速度 */
    public static maxSpeed = 200;
    /**代理对象的初始速度 */
    public static velocity = new Vector2(0, 0);
    /**最大邻居数 */
    public static maxNeighbors = 20;
    /**安全单位时间，它乘以最大速度就是agent的避让探针，值越大，就会越早做出避让行为 */
    public static timeHorizon = 25;
    /**与timeHorizon类似，只针对障碍物 */
    public static timeHorizonObst = 5;

    /**步骤帧 */
    public static rvoTimeStep = 1;
}


export default class RVO
{
    private static _instance: RVO;
    public static get instance()
    {
        if (!RVO._instance)
        {
            RVO._instance = new RVO();
        }
        return RVO._instance;
    }

    public static s_totalID = 0;

    public agentNo2indexDict_: Map<number, number>;
    public index2agentNoDict_: Map<number, number>;

    public agents_: Array<Agent> = [];
    public obstacles_: Array<Obstacle> = [];
    public kdTree_: KdTree;
    public timeStep_: number;

    private defaultAgent_: Agent;
    private globalTime_: number;

    constructor()
    {
        this.init();
    }

    private init()
    {
        this.agents_ = [];
        this.agentNo2indexDict_ = new Map();
        this.index2agentNoDict_ = new Map();
        this.defaultAgent_ = null;
        this.kdTree_ = new KdTree();
        this.obstacles_ = [];
        this.globalTime_ = 0;
        this.timeStep_ = 0.1;

    }

    public doStep()
    {
        this.updateDeleteAgent();

        this.kdTree_.buildAgentTree();

        for (let i = 0, j = this.agents_.length; i < j; i++)
        {
            let agent = this.agents_[i];
            agent.computeNeighbors();
            agent.computeNewVelocity();
        }

        for (let i = 0, j = this.agents_.length; i < j; i++)
        {
            let agent = this.agents_[i];
            agent.update();
        }

        this.globalTime_ += this.timeStep_;
        return this.globalTime_;
    }

    private updateDeleteAgent()
    {
        let isDelete = false;
        for (let i = this.agents_.length - 1; i >= 0; i--)
        {
            if (this.agents_[i].needDelete_)
            {
                this.agents_.splice(i, 1);
                isDelete = true;
            }
        }
        if (isDelete)
            this.onDelAgent();
    }

    public addAgent(position: Vector2)
    {
        if (this.defaultAgent_ == null) return -1;
        let agent = new Agent();
        agent.id_ = RVO.s_totalID;
        RVO.s_totalID++;
        agent.maxNeighbors_ = this.defaultAgent_.maxNeighbors_;
        agent.maxSpeed_ = this.defaultAgent_.maxSpeed_;
        agent.neighborDist_ = this.defaultAgent_.neighborDist_;
        agent.position_ = position;
        agent.radius_ = this.defaultAgent_.radius_;
        agent.timeHorizon_ = this.defaultAgent_.timeHorizon_;
        agent.timeHorizonObst_ = this.defaultAgent_.timeHorizonObst_;
        agent.velocity_ = this.defaultAgent_.velocity_;
        this.agents_.push(agent);
        this.onAddAgent();
        return agent.id_;
    }

    public delAgent(sid:number) {
        let index = this.agentNo2indexDict_.get(sid);
        this.agents_[index].needDelete_ = true;
        // for (let i = 0; i < this.agents_.length; i++) {
        //     if (this.agents_[i].id_ == sid) {
        //         this.agents_.splice(i, 1);
        //     }
        // }
    }

    public addObstacle(vertices: Array<Vector2>)
    {
        if (vertices.length < 2) return -1;

        let obstacleNo = this.obstacles_.length;
        for (let i = 0; i < vertices.length; ++i)
        {
            let obstacle = new Obstacle();
            obstacle.point_ = vertices[i];

            if (i != 0)
            {
                obstacle.previous_ = this.obstacles_[this.obstacles_.length - 1];
                obstacle.previous_.next_ = obstacle;
            }

            if (i == vertices.length - 1)
            {
                obstacle.next_ = this.obstacles_[obstacleNo];
                obstacle.next_.previous_ = obstacle;
            }

            obstacle.direction_ = RVOMath.normalize(Vector2.subtract(vertices[(i == vertices.length - 1 ? 0 : i + 1)], vertices[i]));

            if (vertices.length == 2)
            {
                obstacle.convex_ = true;
            } else
            {
                obstacle.convex_ = (RVOMath.leftOf(vertices[(i == 0 ? vertices.length - 1 : i - 1)], vertices[i], vertices[(i == vertices.length - 1 ? 0 : i + 1)]) >= 0);
            }

            obstacle.id_ = this.obstacles_.length;
            this.obstacles_.push(obstacle);
        }

        return obstacleNo;
    }

    private onDelAgent()
    {
        this.agentNo2indexDict_.clear();
        this.index2agentNoDict_.clear();

        for (let i = 0; i < this.agents_.length; i++)
        {
            let agentNo = this.agents_[i].id_;
            this.agentNo2indexDict_.set(agentNo, i);
            this.index2agentNoDict_.set(i, agentNo);
        }
    }

    private onAddAgent()
    {
        if (this.agents_.length == 0)
            return;

        let index = this.agents_.length - 1;
        let agentNo = this.agents_[index].id_;
        this.agentNo2indexDict_.set(agentNo, index);
        this.index2agentNoDict_.set(index, agentNo);
    }

    public getAgentPosition(agentNo: number)
    {
        let agent = this.agents_[this.agentNo2indexDict_.get(agentNo)];
        if (agent)
        {
            return agent.position_;
        } else
        {
            return new Vector2(0, 0);
        }
    }

    public getAgent(agentNo:number):Agent {
        return this.agents_[this.agentNo2indexDict_.get(agentNo)];
    }

    public getAgentPrefVelocity(agentNo: number)
    {
        return this.agents_[this.agentNo2indexDict_.get(agentNo)].prefVelocity_;
    }

    public setTimeStep(timeStep: number)
    {
        this.timeStep_ = timeStep;
    }

    public setAgentDefaults(neighborDist: number, maxNeighbors: number, timeHorizon: number, timeHorizonObst: number, radius: number, maxSpeed: number, velocity: Vector2)
    {
        if (this.defaultAgent_ == null)
        {
            this.defaultAgent_ = new Agent();
        }

        this.defaultAgent_.maxNeighbors_ = maxNeighbors;
        this.defaultAgent_.maxSpeed_ = maxSpeed;
        this.defaultAgent_.neighborDist_ = neighborDist;
        this.defaultAgent_.radius_ = radius;
        this.defaultAgent_.timeHorizon_ = timeHorizon;
        this.defaultAgent_.timeHorizonObst_ = timeHorizonObst;
        this.defaultAgent_.velocity_ = velocity;
    }

    public processObstacles()
    {
        this.kdTree_.buildObstacleTree();
    }

    public setAgentPrefVelocity(agentNo: number, prefVelocity: Vector2)
    {
        this.agents_[this.agentNo2indexDict_.get(agentNo)].prefVelocity_ = prefVelocity;
    }
}